﻿#include <iostream>
#include <fstream>
#include <vector>
#include <thread>
#include <mutex>
#include <condition_variable>
#include <chrono>
#include <random>
#include "CircularBuffer.h"
#include "WIT_Argeement.h"
using namespace std;

std::mutex mtx;  // 全局互斥锁
CircularBuffer cb;

// 线程A：从文件读取数据
void threadA_reader(const std::string& filename) {
    std::ifstream file(filename, std::ios::binary);
    if (!file) {
        std::cerr << "无法打开文件: " << filename << std::endl;
        return;
    }

    uint8_t chunk[128]; // 每次读取66字节
    while (file) {
        file.read((char*)chunk, 66);

        mtx.lock();
        cb.append(chunk, 66);
        mtx.unlock();

        // 模拟10ms间隔
        std::this_thread::sleep_for(std::chrono::milliseconds(10));
    }

    std::cout << "文件读取完成" << std::endl;
}

// 线程B：处理数据
void threadB_processor() {
    std::random_device rd;
    std::mt19937 gen(rd());
    std::uniform_int_distribution<> dist(5, 20); // 5-20ms随机间隔

    std::ofstream theOutFile;
    theOutFile.open ("result3.csv", ios::out);

    uint8_t dataBuffer[128] = { 0 };
    uint8_t target[2] = { 0x55, 0x50 };
    
    time_t timeBase = time(NULL);
    time_t timeCurrent = time(NULL);

    char writeBuffer[512] = { 0 };
    size_t writeLength = 0;
    //表头
    writeLength = sprintf_s(writeBuffer,
        "Timestamp,"
        "Acceleration_X,Acceleration_Y,Acceleration_Z,"
        "Gyroscope_X,Gyroscope_Y,Gyroscope_Z,"
        "Euler_Angle_Pitch,Euler_Angle_Roll,Euler_Angle_Yaw,"
        "Magnetic_Flux_Density_X,Magnetic_Flux_Density_Y,Magnetic_Flux_Density_Z,"
        "Quaternions_W,Quaternions_X,Quaternions_Y,Quaternions_Z,\n");
    theOutFile.write(writeBuffer, writeLength);
    //单位
    writeLength = sprintf_s(writeBuffer,
        "---,"
        "m/(s^2),m/(s^2),m/(s^2),"
        "deg/s,deg/s,deg/s,"
        "Degrees,Degrees,Degrees,"
        "mT,mT,mT,"
        "no_units,no_units,no_units,no_units,\n");
    theOutFile.write(writeBuffer, writeLength);

    theOutFile.flush();

    bool flag = false;
    uint8_t* result = nullptr;
    while (true) {
        
        // 随机休眠
        std::this_thread::sleep_for(std::chrono::milliseconds(dist(gen)));

        if (flag == false)
        {
            mtx.lock();

            result = cb.find(target, 2);
            if (result != nullptr)
            {
                if (cb.eraseFront(result))
                {
                    if (cb.consume(dataBuffer, 66))
                    {
                        flag = true;
                        timeBase = time(NULL);
                    }
                }
            }
            mtx.unlock();
        }
        

        if (flag == true)
        {
            if (WIT::IMUdataSprintf(dataBuffer, writeBuffer, &writeLength))
            {
                theOutFile.write(writeBuffer, writeLength);
                theOutFile.flush();
            }
            flag = false;
        }

        timeCurrent = time(NULL);
        if (timeCurrent - timeBase > 10)//连续10s没有数据，跳出循环
            break;
    }
    theOutFile.close();
}

void test1()
{
    uint8_t data1[66] = {
    0x55, 0x50, 0x19, 0x04, 0x07, 0x0C, 0x08, 0x00, 0xD9, 0x02, 0xB8,
    0x55, 0x51, 0x0E, 0x00, 0x33, 0x00, 0x03, 0x08, 0x84, 0x09, 0x7F,
    0x55, 0x52, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x84, 0x09, 0x34,
    0x55, 0x53, 0x0A, 0x01, 0xBC, 0xFF, 0x9F, 0x42, 0x0B, 0x82, 0xDC,
    0x55, 0x54, 0xAB, 0x04, 0x5D, 0xFE, 0x69, 0xE0, 0x00, 0x00, 0xFC,
    0x55, 0x59, 0x89, 0x57, 0x6C, 0x01, 0xE7, 0x00, 0x5E, 0x5D, 0x9D
    };
    uint8_t data2[66] = {
    0x55, 0x50, 0x19, 0x04, 0x07, 0x0C, 0x0D, 0x1F, 0xE9, 0x01, 0xEB,
    0x55, 0x51, 0xBC, 0xFE, 0x63, 0x07, 0x4D, 0x05, 0x39, 0x0C, 0x61,
    0x55, 0x52, 0x41, 0x0A, 0xB4, 0x04, 0xDA, 0xFE, 0x39, 0x0C, 0xC7,
    0x55, 0x53, 0x56, 0x2C, 0x91, 0x0C, 0x8C, 0x76, 0x0B, 0x82, 0x56,
    0x55, 0x54, 0x27, 0x0B, 0x76, 0xE6, 0x42, 0xF8, 0x00, 0x00, 0x71,
    0x55, 0x59, 0xA1, 0x16, 0xDE, 0xF6, 0xFA, 0x42, 0x4E, 0x6A, 0x2D
    };
    uint8_t data3[66] = {
    0x55, 0x50, 0x19, 0x04, 0x07, 0x0C, 0x0D, 0x1F, 0xD3, 0x03, 0xD7,
    0x55, 0x51, 0xA3, 0xFE, 0x09, 0x04, 0x06, 0x07, 0x38, 0x0C, 0xA5,
    0x55, 0x52, 0x5D, 0xFE, 0xA3, 0xFF, 0x9F, 0x00, 0x38, 0x0C, 0x87,
    0x55, 0x53, 0xC1, 0x13, 0xC5, 0x07, 0xC7, 0x6D, 0x0B, 0x82, 0x09,
    0x55, 0x54, 0xC7, 0x08, 0x6A, 0xEE, 0x88, 0xE8, 0x00, 0x00, 0x40,
    0x55, 0x59, 0x46, 0x1E, 0x40, 0xFB, 0x73, 0x20, 0xF6, 0x77, 0x4D
    };
    uint8_t data4[] = {
        0x55, 0x50, 0x19, 0x05, 0x03, 0x0B, 0x15, 0x10, 0xE3, 0x03, 0xDC,
        0x55, 0x51, 0x48, 0x00, 0xE1, 0xFF, 0x36, 0x08, 0x21, 0x0A, 0x37,
        0x55, 0x52, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x21, 0x0A, 0xD2,
        0x55, 0x53, 0x68, 0xFF, 0x9A, 0xFE, 0xB7, 0x8D, 0x0B, 0x82, 0x78,
        0x55, 0x54, 0xA7, 0xF6, 0xE2, 0xEB, 0x70, 0xDE, 0x00, 0x00, 0x61,
        0x55, 0x59, 0x6B, 0x15, 0xAD, 0xFD, 0x8D, 0x00, 0xD5, 0x81, 0xBB
    };
    uint8_t data5[] = {
        0x55, 0x50, 0x19, 0x05, 0x03, 0x0B, 0x15, 0x28, 0xB7, 0x02, 0xC7,
        0x55, 0x51, 0x48, 0x00, 0xE0, 0xFF, 0x32, 0x08, 0x73, 0x0A, 0x84,
        0x55, 0x52, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x73, 0x0A, 0x24,
        0x55, 0x53, 0x63, 0xFF, 0x96, 0xFE, 0xB7, 0x8D, 0x0B, 0x82, 0x6F,
        0x55, 0x54, 0x2D, 0xF8, 0xD0, 0xEB, 0x9F, 0xDE, 0x00, 0x00, 0x06,
        0x55, 0x59, 0x6A, 0x15, 0xA6, 0xFD, 0x94, 0x00, 0xD5, 0x81, 0xBA
    };    
    uint8_t data6[] = {
        0x55, 0x50, 0x19, 0x05, 0x03, 0x0B, 0x15, 0x29, 0x53, 0x02, 0x64,
        0x55, 0x51, 0x48, 0x00, 0xE0, 0xFF, 0x34, 0x08, 0x76, 0x0A, 0x89,
        0x55, 0x52, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x76, 0x0A, 0x27,
        0x55, 0x53, 0x63, 0xFF, 0x96, 0xFE, 0xB7, 0x8D, 0x0B, 0x82, 0x6F,
        0x55, 0x54, 0x93, 0xF6, 0xCD, 0xEB, 0x72, 0xDE, 0x00, 0x00, 0x3A,
        0x55, 0x59, 0x6A, 0x15, 0xA6, 0xFD, 0x94, 0x00, 0xD5, 0x81, 0xBA
    };    
    uint8_t data7[] = {
        0x55, 0x50, 0x19, 0x05, 0x03, 0x0B, 0x15, 0x29, 0xE3, 0x03, 0xF5,
        0x55, 0x51, 0x49, 0x00, 0xE0, 0xFF, 0x35, 0x08, 0x74, 0x0A, 0x89,
        0x55, 0x52, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x74, 0x0A, 0x25,
        0x55, 0x53, 0x63, 0xFF, 0x96, 0xFE, 0xB7, 0x8D, 0x0B, 0x82, 0x6F,
        0x55, 0x54, 0x6A, 0xF6, 0xDF, 0xEB, 0x8B, 0xDE, 0x00, 0x00, 0x3C,
        0x55, 0x59, 0x6A, 0x15, 0xA5, 0xFD, 0x94, 0x00, 0xD5, 0x81, 0xB9
    };


    char writeBuffer[256] = { 0 };
    size_t length = 0;
    //if (WIT::IMUdataSprintf(data1, writeBuffer, &length))
    //    cout.write(writeBuffer, length);//打印特定长度
    //if (WIT::IMUdataSprintf(data2, writeBuffer, &length))
    //    cout.write(writeBuffer, length);//打印特定长度
    //if (WIT::IMUdataSprintf(data3, writeBuffer, &length))
    //    cout.write(writeBuffer, length);//打印特定长度

    CircularBuffer cb;
    uint8_t dataBuffer[128];

    cb.append(data1, 66);
    cb.consume(dataBuffer, 66);
    if (WIT::IMUdataSprintf(dataBuffer, writeBuffer, &length))
        cout.write(writeBuffer, length);
    cout << endl;

    cb.append(data2, 66);
    cb.consume(dataBuffer, 66);
    if (WIT::IMUdataSprintf(dataBuffer, writeBuffer, &length))
        cout.write(writeBuffer, length);
    cout << endl;

    cb.append(data3, 66);
    cb.consume(dataBuffer, 66);
    if (WIT::IMUdataSprintf(dataBuffer, writeBuffer, &length))
        cout.write(writeBuffer, length);
    cout << endl;

    cb.append(data4, 66);
    cb.consume(dataBuffer, 66);
    if (WIT::IMUdataSprintf(dataBuffer, writeBuffer, &length))
        cout.write(writeBuffer, length);
    cout << endl;

    cb.append(data5, 66);
    cb.consume(dataBuffer, 66);
    if (WIT::IMUdataSprintf(dataBuffer, writeBuffer, &length))
        cout.write(writeBuffer, length);
    cout << endl;

    cb.append(data6, 66);
    cb.consume(dataBuffer, 66);
    if (WIT::IMUdataSprintf(dataBuffer, writeBuffer, &length))
        cout.write(writeBuffer, length);
    cout << endl;

    cb.append(data7, 66);
    cb.consume(dataBuffer, 66);
    if (WIT::IMUdataSprintf(dataBuffer, writeBuffer, &length))
        cout.write(writeBuffer, length);
    cout << endl;
}

void test2()
{
    std::ofstream theOutFile;
    theOutFile.open("result.csv", ios::out);


    time_t timeBase = time(NULL);
    time_t timeCurrent = time(NULL);

    char writeBuffer[512] = { 0 };
    size_t writeLength = 0;
    //表头
    writeLength = sprintf_s(writeBuffer,
        "Timestamp,"
        "Acceleration_X,Acceleration_Y,Acceleration_Z,"
        "Gyroscope_X,Gyroscope_Y,Gyroscope_Z,"
        "Euler_Angle_Pitch,Euler_Angle_Roll,Euler_Angle_Yaw,"
        "Magnetic_Flux_Density_X,Magnetic_Flux_Density_Y,Magnetic_Flux_Density_Z,"
        "Quaternions_W,Quaternions_X,Quaternions_Y,Quaternions_Z,\n");
    theOutFile.write(writeBuffer, writeLength);
    //单位
    writeLength = sprintf_s(writeBuffer,
        "---,"
        "m/(s^2),m/(s^2),m/(s^2),"
        "deg/s,deg/s,deg/s,"
        "Degrees,Degrees,Degrees,"
        "mT,mT,mT,"
        "no_units,no_units,no_units,no_units,\n");
    theOutFile.write(writeBuffer, writeLength);

    theOutFile.flush();


    std::ifstream file("imu_data.bin", std::ios::binary);
    if (!file) {
        std::cerr << "无法打开文件: " << "imu_data.bin" << std::endl;
    }

    uint8_t chunk[128]; // 每次读取66字节
    CircularBuffer cb;
    uint8_t dataBuffer[128];
    uint8_t target[2] = { 0x55, 0x50 };
    uint8_t* result;

    size_t whileCount = 0;
    size_t appendCount = 0;
    size_t eraseCount = 0;
    size_t consumeCount = 0;

    while (file) {
        whileCount++;
        /*std::cout << "第 " << whileCount << " 次循环" << std::endl;*/
        /*cb.printfCircularBuffer();*/

        file.read((char*)chunk, 66);
        
        appendCount++;
       /* std::cout << "第 " << appendCount << " 次追加" << std::endl;*/
        cb.append(chunk, 66);
        /*cb.printfCircularBuffer();*/

        result = cb.find(target, 2);
        if (result!=nullptr)
        {
            if (cb.eraseFront(result))
            {
                eraseCount++;
                /*std::cout << "第 " << eraseCount << " 次删除头部无效数据" << std::endl;*/
                /*cb.printfCircularBuffer();*/
                if (cb.consume(dataBuffer, 66)==true)
                {
                    consumeCount++;
                    /*std::cout << "第 " << consumeCount << " 次消费" << std::endl;*/
                    /*cb.printfCircularBuffer();*/
                    if (WIT::IMUdataSprintf(dataBuffer, writeBuffer, &writeLength))
                    {
                        theOutFile.write(writeBuffer, writeLength);
                        //theOutFile.flush();//调试用
                        
                    }
                }
            }
            
        }
        
    }

    file.close();
    theOutFile.close();
}

void test3()
{
    const std::string filename = "imu_data.bin";    

    // 启动线程
    std::thread t1(threadA_reader, filename);
    std::thread t2(threadB_processor);

    t1.join();
    t2.join();
}

int main() {
    

    /*test1();*/
    test2();
    /*test3();*/

    return 0;
}